/* main.c -- arquivo main
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogofsr@grad.icmc.usp.br>
 *                     Bruno Ribeiro de Oliveira
 *                     Arineiza Cristina Pinheiro
 *                     Tomas Grover
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/** @file
 *  
 *  Arquivo main onde a lógica acontece.
 */

#include <stdlib.h>
#include <pthread.h>
#include <unistd.h>

#include "roboboca.h"
#include "xerror.h"

/* Static Functions */
static void *robot1_run( void *ptr );
static void *robot2_run( void *ptr );
static void *robot3_run( void *ptr );

/** Função Main
 *
 * É aqui que a mágica começa.
 *
 */
int
main( int argc, char *argv[] )
{
    roboboca_t *box = NULL;
    roboboca_robot_t *robot1 = NULL;
    roboboca_robot_t *robot2 = NULL;
    roboboca_robot_t *robot3 = NULL;
    pthread_t thread_robot1;
    pthread_t thread_robot2;
    pthread_t thread_robot3;
    int iret1 = 0;
    int iret2 = 0;
    int iret3 = 0;    
    const char *end_server = {"localhost"};

    /* Criando estrutura que segura todos os robôs */
    box = roboboca_new( 3 );

    /* Robô 1 azul */
/*    if( (robot1 = roboboca_robot_new( -11,
				      -1,
				      end_server,
				      6665 )) == NULL )
*/
   /* Robô 1 amarelo */
    if( (robot1 = roboboca_robot_new( 11,
				      1,
				      end_server,
				      6668)) == NULL )

    {
	roboboca_destroy(box);
	xerror_fatal("Servidor inacessivel. PARANDO AGORA!");
    }
    roboboca_append_robot( box,
			   robot1 );

    /* Robô 2 azul */
/*    if( (robot2 = roboboca_robot_new( -11,
				     0,
				     end_server,
				     6666 )) == NULL )
*/
   /* Robô 2 amarelo */
    if( (robot2 = roboboca_robot_new( 11,
				      0,
				      end_server,
				      6669 )) == NULL )

    {
	roboboca_destroy(box);
	xerror_fatal("Servidor inacessivel. PARANDO AGORA!");
    }
    roboboca_append_robot( box,
			   robot2 );

    /* Robô 3 azul */
/*    if( (robot3 = roboboca_robot_new( -11,
				      1,
				      end_server,
				      6667 )) == NULL )
*/
   /* Robô 3 amarelo */
    if( (robot3 = roboboca_robot_new( 11,
				      1,
				      end_server,
				      6670 )) == NULL )

    {
	roboboca_destroy(box);
	xerror_fatal("Servidor inacessivel. PARANDO AGORA!");
    }
    roboboca_append_robot( box,
			   robot3 );

    /* Soltando as threads de cada robô. A partir daqui é cada um por
     * si. */
    iret1 = pthread_create( &thread_robot1,
			    NULL,
			    robot1_run,
			    (void*) box );
    iret2 = pthread_create( &thread_robot2,
			    NULL,
			    robot2_run,
			    (void*) box );
    iret3 = pthread_create( &thread_robot3,
			    NULL,
			    robot3_run,
			    (void*) box );
    pthread_join( thread_robot1,
		  NULL );
    pthread_join( thread_robot2,
		  NULL );
    pthread_join( thread_robot3,
		  NULL );

    roboboca_destroy( box );
    return EXIT_SUCCESS;
}

/** Comanda o robô 1.
 *
 * Função iniciada depois de lançada a thread do robô 1.
 p*
 * @param *ptr Ponteiro para a caixa de robôs.
 */
void
*robot1_run( void *ptr )
{
    roboboca_t *box = (roboboca_t *) ptr;
    roboboca_robot_t *robot = box->l_robots[0];

    int blob_i;

    while( box->all_blobs_returned == 0 )
    {
	while( box->all_blobs_finded == 0 )
	{
	   /* Robô 1 azul */
/*	   
	    roboboca_goto_simple (box, robot, -11, -5.5, 1);
	    roboboca_goto_simple (box, robot, 11, -4, 1);
	    roboboca_goto_simple (box, robot, 6, -7, 1);
*/
      /* Robô 1 amarelo */

	    roboboca_goto_simple (box, robot, 9, 6, 1);
	    roboboca_goto_simple (box, robot, -7, 3, 1);
 	    roboboca_goto_simple (box, robot, -10, 6, 1); 	    

	    roboboca_goto_simple (box, robot, robot->x_orig, robot->y_orig, 1);
	}
	
	blob_i = roboboca_get_nearest_blob( box, robot );

	if( blob_i == -1 )
	{
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0,
					    0,
					    0,
					    1);
	    sleep(2);
	}
	else
	{
	    if ( roboboca_robot_grab_blob( box, robot, blob_i ) == EXIT_SUCCESS )
	    {
		roboboca_robot_return_blob( box, robot, blob_i );
	    }
	}
    }

    return NULL;
}

/** Comanda o robô 2.
 *
 
 * Função iniciada depois de lançada a thread do robô 2.
 *
 * @param *ptr Ponteiro para a caixa de robôs.
 */
void
*robot2_run( void *ptr )
{
    roboboca_t *box = (roboboca_t *) ptr;
    roboboca_robot_t *robot = box->l_robots[1];
    int blob_i;

    while( box->all_blobs_returned == 0 )
    {
	while( box->all_blobs_finded == 0 )
	{
	   /* Robô 2 azul */
/*
	    roboboca_goto_simple (box, robot, 10, 0, 1);
*/
	   /* Robô 2 amarelo */

	    roboboca_goto_simple (box, robot, -10, 0, 1);	    

	    roboboca_goto_simple (box, robot, robot->x_orig, robot->y_orig, 1);
	}
	
	blob_i = roboboca_get_nearest_blob( box, robot );

	if( blob_i == -1 )
	{
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0,
					    0,
					    0,
					    1);
	    sleep(2);
	}
	else
	{
	    if ( roboboca_robot_grab_blob( box, robot, blob_i ) == EXIT_SUCCESS )
	    {
		roboboca_robot_return_blob( box, robot, blob_i );
	    }
	}
    }
    
    return NULL;
}

/** Comanda o robô 3.
 *
 * Função iniciada depois de lançada a thread do robô 3.
 *
 * @param *ptr Ponteiro para a caixa de robôs.
 */
void
*robot3_run( void *ptr )
{
    roboboca_t *box = (roboboca_t *) ptr;
    roboboca_robot_t *robot = box->l_robots[2];

    int blob_i;

    while( box->all_blobs_returned == 0 )
    {
	while( box->all_blobs_finded == 0 )
	{	
	   /* Robô 3 azul */	
/*
	    roboboca_goto_simple (box, robot, -10, 6, 1);
	    roboboca_goto_simple (box, robot, 8, 5, 1);
*/
	   /* Robô 3 amarelo */	    

	    roboboca_goto_simple (box, robot, 10, -5, 1);
       roboboca_goto_simple (box, robot, -10, -6, 1);

	    roboboca_goto_simple (box, robot, robot->x_orig, robot->y_orig, 1);
	}
	
	blob_i = roboboca_get_nearest_blob( box, robot );

	if( blob_i == -1 )
	{
	    playerc_position2d_set_cmd_vel( robot->position2d,
					    0,
					    0,
					    0,
					    1);
	    sleep(2);
	}
	else
	{
	    if ( roboboca_robot_grab_blob( box, robot, blob_i ) == EXIT_SUCCESS )
	    {
		roboboca_robot_return_blob( box, robot, blob_i );
	    }
	}
    }
    
    return NULL;
}
